/**
@file
@brief 
*/
#include "MotorControl.h"
#include "HBridge.h"

/* ===== ===== ===== Private Functions ===== ===== =====*/
static void HBridge_TimInit(void) {
    RCC_ClocksTypeDef st_clk;
    TIM_TimeBaseInitTypeDef st_tb;
    TIM_OCInitTypeDef st_oc;
    uint32_t timx_freq;
//    
//    // ===== Calculate TIMxCLK =====
//    // refer to RM0008 figure 8. clock tree
    RCC_GetClocksFreq(&st_clk);
    if (st_clk.HCLK_Frequency == st_clk.PCLK2_Frequency) {
        timx_freq = st_clk.PCLK2_Frequency;
    }
    else {
        timx_freq = st_clk.PCLK2_Frequency * 2;
    }
//    timx_freq = 1;  // ??? this statement cause pwm wave gone
    
    // ===== Config Timebase =====
    st_tb.TIM_Period = timx_freq/MC_PWM_FREQ - 1;
    st_tb.TIM_Prescaler = 0;
    st_tb.TIM_ClockDivision = TIM_CKD_DIV1;
    st_tb.TIM_CounterMode = TIM_CounterMode_Up;
    TIM_TimeBaseInit(TIM_HB_A, &st_tb);
    TIM_TimeBaseInit(TIM_HB_B, &st_tb);
//    
//    // ===== PWM Config =====
    st_oc.TIM_OCMode = TIM_OCMode_PWM1;
    st_oc.TIM_OutputState = TIM_OutputState_Enable;
    st_oc.TIM_Pulse = 0;
    st_oc.TIM_OCPolarity = TIM_OCPolarity_High;
    
    // cofig that not used, but have to be filled
    st_oc.TIM_OCIdleState = TIM_OCIdleState_Set;
    st_oc.TIM_OCNIdleState = TIM_OCNIdleState_Set;
    st_oc.TIM_OCNPolarity = TIM_OCNPolarity_High;
    st_oc.TIM_OutputNState = TIM_OutputNState_Disable;
   
    // Motor 1
    TIM_OC1Init(TIM_HB_A, &st_oc);
    TIM_OC1PreloadConfig(TIM_HB_A, TIM_OCPreload_Enable);

    TIM_OC1Init(TIM_HB_B, &st_oc);
    TIM_OC1PreloadConfig(TIM_HB_B, TIM_OCPreload_Enable);
    
    // Motor 2
    TIM_OC2Init(TIM_HB_A, &st_oc);
    TIM_OC2PreloadConfig(TIM_HB_A, TIM_OCPreload_Enable);

    TIM_OC2Init(TIM_HB_B, &st_oc);
    TIM_OC2PreloadConfig(TIM_HB_B, TIM_OCPreload_Enable);
    
    // Motor 3
    TIM_OC3Init(TIM_HB_A, &st_oc);
    TIM_OC3PreloadConfig(TIM_HB_A, TIM_OCPreload_Enable);

    TIM_OC3Init(TIM_HB_B, &st_oc);
    TIM_OC3PreloadConfig(TIM_HB_B, TIM_OCPreload_Enable);
    
    // Motor 4
    TIM_OC4Init(TIM_HB_A, &st_oc);
    TIM_OC4PreloadConfig(TIM_HB_A, TIM_OCPreload_Enable);

    TIM_OC4Init(TIM_HB_B, &st_oc);
    TIM_OC4PreloadConfig(TIM_HB_B, TIM_OCPreload_Enable);
    
//    // ===== Enable =====
    TIM_CtrlPWMOutputs(TIM_HB_A,ENABLE);
    TIM_ARRPreloadConfig(TIM_HB_A, ENABLE);
    TIM_Cmd(TIM_HB_A, ENABLE);
    
    TIM_CtrlPWMOutputs(TIM_HB_B,ENABLE);
    TIM_ARRPreloadConfig(TIM_HB_B, ENABLE);
    TIM_Cmd(TIM_HB_B, ENABLE);
}



/**@brief Set H-Bridge to Run state. */
static void HBridge_StateRun(volatile HBridge_TypeDef* hb) {
    // ===== ===== Apply DirMode =====
    // Output on each condition:
    //  _____________________________
    // | DirMode | Dir | OutA | OutB |
    // |---------|-----|------|------|
    // |  AHBL   |  F  |      |      |
    // |---------|-----| PWM  |  L   |
    // |  ALBH   |  B  |      |      |
    // |---------|-----|------|------|
    // |  AHBL   |  B  |      |      |
    // |---------|-----|  L   | PWM  |
    // |  ALBH   |  F  |      |      |
    // |---------|-----|------|------|
    //
    // Note: NPWM stands for PWM with inversed polarity.
    
    // Set GPIO 
    GPIO_InitTypeDef st_g;
    st_g.GPIO_Speed = GPIO_Speed_50MHz;
    if (
         ((HB_DirMode_AHBLasForward == hb->Param.DirMode) && (HB_Dir_Forward == hb->Param.Dir))
       ||((HB_DirMode_ALBHasForward == hb->Param.DirMode) && (HB_Dir_Backward == hb->Param.Dir))
       ) 
    {
        // output PWM
        st_g.GPIO_Mode = GPIO_Mode_AF_PP;
        st_g.GPIO_Pin = hb->Hw.Pin_InA;
        GPIO_Init(hb->Hw.GpioPort_InA, &st_g);
        
        // output low level
        st_g.GPIO_Mode = GPIO_Mode_Out_PP;
        st_g.GPIO_Pin = hb->Hw.Pin_InB;
        GPIO_ResetBits(hb->Hw.GpioPort_InB, hb->Hw.Pin_InB);
        GPIO_Init(hb->Hw.GpioPort_InB, &st_g);
    }
    else {
        // output low level
        st_g.GPIO_Mode = GPIO_Mode_Out_PP;
        st_g.GPIO_Pin = hb->Hw.Pin_InA;
        GPIO_ResetBits(hb->Hw.GpioPort_InA, hb->Hw.Pin_InA);
        GPIO_Init(hb->Hw.GpioPort_InA, &st_g);
        
        // output PWM
        st_g.GPIO_Mode = GPIO_Mode_AF_PP;
        st_g.GPIO_Pin = hb->Hw.Pin_InB;
        GPIO_Init(hb->Hw.GpioPort_InB, &st_g);
    }
}

/**@brief Set H-Bridge to Brake state. */
static void HBridge_StateBrake(volatile HBridge_TypeDef* hb) {
    // Set both PWMxA, PWMxB to high
    GPIO_InitTypeDef _st_g;
    
    _st_g.GPIO_Mode = GPIO_Mode_Out_PP;
    _st_g.GPIO_Speed = GPIO_Speed_50MHz;
    
    _st_g.GPIO_Pin = hb->Hw.Pin_InA;
    GPIO_Init(hb->Hw.GpioPort_InA, &_st_g);
    GPIO_SetBits(hb->Hw.GpioPort_InA, hb->Hw.Pin_InA);
    
    _st_g.GPIO_Pin = hb->Hw.Pin_InB;
    GPIO_Init(hb->Hw.GpioPort_InB, &_st_g);
    GPIO_SetBits(hb->Hw.GpioPort_InB, hb->Hw.Pin_InB);
}

/**@brief Set H-Bridge to Coast state. */
static void HBridge_StateCoast(volatile HBridge_TypeDef* hb) {
    // Set both PWMxA, PWMxB to low
    GPIO_InitTypeDef _st_g;
    
    _st_g.GPIO_Mode = GPIO_Mode_Out_PP;
    _st_g.GPIO_Speed = GPIO_Speed_50MHz;
    
    _st_g.GPIO_Pin = hb->Hw.Pin_InA;
    GPIO_Init(hb->Hw.GpioPort_InA, &_st_g);
    GPIO_ResetBits(hb->Hw.GpioPort_InA, hb->Hw.Pin_InA);
    
    _st_g.GPIO_Pin = hb->Hw.Pin_InB;
    GPIO_Init(hb->Hw.GpioPort_InB, &_st_g);
    GPIO_ResetBits(hb->Hw.GpioPort_InB, hb->Hw.Pin_InB);
}


/** @brief Set H-Bridge to specific operating state. */
static void HBridge_SetState(volatile HBridge_TypeDef* hb) {
    switch(hb->Param.State) {
        case HB_State_Run:
            HBridge_StateRun(hb); break;
        case HB_State_Brake:
            HBridge_StateBrake(hb); break;
        case HB_State_Coast:
            HBridge_StateCoast(hb); break;
        default: break;
    }
}




/* ===== ===== ===== Exported Functions ===== ===== =====*/
void HBridge_Init(void) {
//    HBridge_GpioInit();
    HBridge_TimInit();
    
    HBridge_StateBrake(&hstHBdg_1);
    HBridge_StateBrake(&hstHBdg_2);
    HBridge_StateBrake(&hstHBdg_3);
    HBridge_StateBrake(&hstHBdg_4);
}

/**@brief Set PWM duty.
   @param hb    The handle to H-Bridge
   @param duty  A value between 0 to 1000
*/
void HBridge_SetPwmDuty(volatile HBridge_TypeDef* hb, uint16_t q10Duty) {
    uint32_t _arr = TIM_HB_A->ARR;
    uint16_t _compare_val;
    
    if (q10Duty > (1 << 10)) {
        q10Duty = 1<< 10;
    }
    _compare_val = q10Duty * _arr >> 10U;
    
    if (hb == (&hstHBdg_1)) {
        TIM_SetCompare1(TIM_HB_A, _compare_val);
        TIM_SetCompare1(TIM_HB_B, _compare_val);
    }
    else if (hb == (&hstHBdg_2)) {
        TIM_SetCompare2(TIM_HB_A, _compare_val);
        TIM_SetCompare2(TIM_HB_B, _compare_val);
    }
    else if (hb == (&hstHBdg_3)) {
        TIM_SetCompare3(TIM_HB_A, _compare_val);
        TIM_SetCompare3(TIM_HB_B, _compare_val);
    }
    else if (hb == (&hstHBdg_4)) {
        TIM_SetCompare4(TIM_HB_A, _compare_val);
        TIM_SetCompare4(TIM_HB_B, _compare_val);
    }
}

/**@brief Apply the H-Bridge parameters to hardware.
*/
void HBridge_Update(volatile HBridge_TypeDef* hb) {
    if (hb->ParamChanged) {
        hb->ParamChanged = false;
        
        HBridge_SetState(hb);
    }
}

